Semi-autonomous vehicles are equipped with systems, such as an assisted parking module, that assist with certain driving tasks when activated by a driver. The assisted parking module assists the driver in sensing a potential parking space, planning a path into the parking space, and exercising lateral, longitudinal and transmission gear control to maneuver the vehicle into the parking space. An accurate estimation of the current vehicle position and heading angle is important for path planning, and lateral and longitudinal control tasks. Traditionally, this is done using odometry and/or a yaw rate sensor. Odometry determines the distance traveled based on the speed of the wheels and the circumference of the wheels.
On narrow streets, parking a vehicle on the street often involves parking so that a portion of the vehicle is on the street and a portion of the vehicle in on the curb. However, curbs can make odometry inaccurate. Thus, curbs cause difficulty for path planning, and lateral and longitudinal control tasks.